//
// Created by  on 12/15/21.
//

#ifndef __SERIAL_DATE_H
#define __SERIAL_DATE_H

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/ByteMultiArray.h>
#include <std_msgs/UInt8MultiArray.h>
#include <std_msgs/Int8.h>
#include <nav_msgs/Odometry.h>
#include <cmath>
#include <vector>
#include <fstream>
#include <iostream>
#include <thread> //线程头文件
#include <stdio.h>
#include <dirent.h>
#include <stdlib.h>
#include <string>
#include <deque>
#include <mutex>  //定义了C++11标准中的一些互斥访问的类与方法
#include <chrono> // std::chrono::seconds
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <ros/duration.h>
#include <cstring>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <time.h>
#include <unistd.h> //sleep fork
#include "CSystemTime.h"
#include "mlog.h"
#include "diagnose.h"
#include "serial_drive.h"
#include <deque>
#include <getopt.h>
#include "timesync_client_uapi.h"
#include "readconfig.hpp"
extern "C" {
#include "ppstest.h"
#include "TwoYearNec.h"
}

union position {
    struct
    {
        double time;
        float rx;
        float ry;
        float rz;
        float x;
        float y;
        float z;
        float raw;
        float pitch;
        float yaw;
    } pos;
    uint8_t bit[44];
};

typedef struct Time_frame {
    uint32_t Sec;
    uint32_t MsSec;
} TIME_frame_t;

union Px30_Mode_Struct {
    struct {
        uint16_t up_car_key : 1;
        uint16_t down_car_key : 1;
        uint16_t work_zone_key : 1;
        uint16_t no_work_zone_key : 1;
        uint16_t low_power_mode : 1;
        uint16_t normal_power_mode : 1;
        uint16_t running_forword_key : 1;
        uint16_t runnin_back_key : 1;
        uint16_t finish_up_down_car_key : 1;
        uint16_t free : 7;
    } detail;
    uint16_t state;
};

union Mlog_Save_Tik {
    struct {
        uint16_t imu : 1;
        uint16_t odom : 1;
        uint16_t rtk : 1;
        uint16_t position : 1;
        uint16_t object : 1;
        uint16_t error : 1;
        uint16_t free : 10;
    } detail;
    uint16_t state;
};

typedef struct uint8_bit {
    uint8_t ucA : 1;
    uint8_t ucB : 1;
    uint8_t ucC : 1;
    uint8_t ucD : 1;
    uint8_t ucE : 1;
    uint8_t ucF : 1;
    uint8_t ucG : 1;
    uint8_t ucH : 1;
} uint8_bit;

typedef union error_bit {
    struct
    {
        uint8_bit u0;
        uint8_bit u1;
        uint8_bit u2;
        uint8_bit u3;
        uint8_bit u4;
        uint8_bit u5;
        uint8_bit u6;
        uint8_bit u7;
    } detail;
    struct
    {
        uint8_t U0;
        uint8_t U1;
        uint8_t U2;
        uint8_t U3;
        uint8_t U4;
        uint8_t U5;
        uint8_t U6;
        uint8_t U7;
    } Detail;
    uint64_t state;
} error_bit;

typedef union {
    struct uint8_bit ibit;
    uint8_t astate;
} ui8_union;

typedef struct GPS_frame {
    uint32_t sec;
    uint32_t usec;
    double lat;
    double lon;
    float hgt;
    uint8_t level;
    float lat_std;
    float lon_std;
} GPS_frame_t;

typedef struct GPS_frame1 {
    float lat_std;
    float lon_std;
} GPS_frame_t1;

typedef struct ODOM_frame {
    int16_t left_encode;
    int16_t right_encode;
} ODOM_frame_t;

typedef struct IMU_frame {
    float acc_x; // linear_acc
    float acc_y;
    float acc_z;
    float gyro_x; // angular_velocity
    float gyro_y;
    float gyro_z;
    float raw;  // raw
    float pich; // pitch
    float yaw;  // yaw
} IMU_frame_t;
typedef struct Time_Counter {
    int header_tik;
    int pos_tik;
    int obj_tik;
    int imu_tik;
    int odom_tik;
    int rtk_tik;
    int temp_tik;
    int rec_tik;
    int error_tik;
} Time_Count;
////data AA 55 00 TIME BB 66 11 RTK CC 77 22 IMU DD 88 33 ODOM
typedef struct Ser_Date_frame {
    const uint8_t H_First = 0xA4;
    const uint8_t H_Second = 0x59;
    const uint8_t H_Third = 0x43;
    uint8_t MSG_FRM = 0x02; // 信号来源
    uint8_t MSG_CTR_F = 0x60;
    uint8_t MSG_CTR_S = 0x01;
    uint8_t Header_Length_Fir = 0xA0;
    uint8_t Header_Length_Sec = 0x00;
    uint8_t Header_Length_Thi = 0x00;
    uint8_t Header_Length_Fth = 0x00;
    const uint8_t MSG_TIME_F = 0xAA;
    const uint8_t MSG_TIME_S = 0x55;
    const uint8_t MSG_TIME_T = 0x00;
    TIME_frame_t MSG_TIM;
    const uint8_t MSG_GPS_F = 0xBB;
    const uint8_t MSG_GPS_S = 0x66;
    const uint8_t MSG_GPS_T = 0x11;
    GPS_frame_t MSG_GPS;
    const uint8_t MSG_IMU_F = 0xCC;
    const uint8_t MSG_IMU_S = 0x77;
    const uint8_t MSG_IMU_T = 0x22;
    IMU_frame_t MSG_IMU;
    const uint8_t MSG_ODOM_F = 0xDD;
    const uint8_t MSG_ODOM_S = 0x88;
    const uint8_t MSG_ODOM_T = 0x33;
    ODOM_frame_t MSG_ODOM;
    const uint8_t MSG_ENDF = 0x0D;
    const uint8_t MSG_ENDS = 0x0A;
} data_frame_t;

typedef union {
    struct
    {
        uint8_t fst;
        ui8_union sec;
        uint8_t thd;
        uint8_t four;
    } detail;
    uint32_t state;
    uint8_t buf[4];
} Alg_State;
typedef union {
    struct {
        uint8_t imu_rtk_odom_;
        uint8_t camera_;
        uint8_t lidar_;
        uint8_t lio_;
        uint8_t vio_;
        uint8_t fusion_;
        uint8_t ai_;
        uint8_t point_cloud_;
        uint8_t ai_seg_;
        uint8_t ai_det_;
    } detail;
    uint8_t buf[10];
} AlgError;

struct
{
    bool ps_time;
    bool pss_time;
    bool ai_time;
    bool ais_time;
} time_sta;
typedef union {
    double time;
    uint8_t bit[8];
} Mytime;

typedef union {
    struct
    {
        uint32_t AiCloud_bit : 1;
        uint32_t Location_bit : 1;
        uint32_t Log_Px30 : 1;
        uint32_t Loging_Px30 : 1;
        uint32_t Ver_Px30 : 1;
        uint32_t WorkIn : 1;
        uint32_t WorkOut : 1;
        uint32_t Updown_bit : 1;
        uint32_t UpCar_px30 : 1;
        uint32_t DownCar_px30 : 1;
        uint32_t FinCar_px30 : 1;
        uint32_t Dir_Cal_bit : 1;
        uint32_t Slam_Reset : 1;
        uint32_t Frt_Px30 : 1;
        uint32_t Bck_Px30 : 1;
        uint32_t Map_Px30 : 1;
        uint32_t Hand_Px30 : 1;
        uint32_t Update_Px30 : 1;
        uint32_t sync_time_px30_tx2 : 1;
        uint32_t px30_mode_ctl : 1;
        uint32_t Ui_Px30 : 1;
        uint32_t DownCaring : 1;
        uint32_t FinCaring : 1;
        uint32_t Send_Pic : 1;
        uint32_t Res : 8;
    } detail;
    uint32_t state;
} Rec_State_Un;

const bool N_SIGNED_YEAR_MOTHER_DAY = false;
// 当无信号时，上传的年月日为0,在程序中需要修补
const uint8_t Msg_Frm_PX30 = 0x00; // 数据来源设备
const uint8_t Msg_Frm_M3 = 0x01;
const uint8_t Msg_Frm_M7 = 0x02;
const uint8_t Msg_Frm_AI = 0x03;
const uint8_t Msg_Cnt_A35AI = 0x60;      // 信主控制类型
const uint8_t Msg_Cnt_fun = 0x01;        // 信息子控制类型
const uint8_t Tx2_upd_sof = 0x15;        // 更新
const uint8_t M7_tx2 = 0x22;             // M7直接传数据到TX2
const uint8_t M7_px3_ai = 0x30;          // M7传数据到PX30到TX2
const uint8_t Tx2_px3_Hand = 0x07;       // TX2与PX30握手
const uint8_t Bld_map_bck = 0x08;        // 建图初始点反馈
const uint8_t Bld_map_init = 0x09;       // 建图初始点
const uint8_t Tx2_p30_UDC = 0x0a;        // 上下车数据发送
const uint8_t p30_Tx2_log = 0x57;        // LOG
const uint8_t Tx2_p30_log = 0x58;        // LOG back
const uint8_t Tx2_p30_Req = 0x9a;        // require
const uint8_t Tx2_p30_RB = 0x9b;         // require_back
const uint8_t Tx2_p30_UI = 0x9c;         // 低功耗
const uint8_t Tx2_p30_Pic = 0x9d;        // 传输图片
const uint8_t PX30_TX2_SYNC_TIME = 0x03; // 时间同步
const uint8_t Alg_Res_Req = 0x01;        // 算法复位请求
const uint8_t Alg_Res = 0x14;            // 算法置位
const uint8_t Dir_Cal_Req = 0x02;        // 行向校准请求
const uint8_t UpCar_Req = 0x03;          // 上车请求
const uint8_t DownCar_Req = 0x04;        // 下车请求
const uint8_t FinCar_Req = 0x05;         // 上下车完成请求
const uint8_t FrtCar_Req = 0x06;         // 前进
const uint8_t BckCar_Req = 0x07;         // 后退
const uint8_t Work_In = 0x08;            // 工作区
const uint8_t Work_Out = 0x09;           // 非工作区
const uint8_t Tx2_Soft_V = 0x62;         // 查询AI设备版本
const uint8_t Tx2_Soft_B = 0x6c;         // 回复TX2版本
const uint8_t Px30_Mode = 0x8a;          // px30发送模式
const uint8_t MSG_H[] = {0xA4, 0x59, 0x43};
const uint8_t MSG_H_GPS[] = {0xBB, 0x66, 0x11};
const uint8_t MSG_H_IMU[] = {0xCC, 0x77, 0x22};
const uint8_t MSG_H_ODOM[] = {0xDD, 0x88, 0x33};
const uint8_t MSG_H_TIME[] = {0xAA, 0x55, 0x00};
const uint8_t MSG_E[] = {0X0D, 0X0A};
const int len_head = 4, len_fun = 2, len_len = sizeof(uint32_t);
const int len_head_data = len_head + len_fun + len_len;
const int len_time = sizeof(uint64_t), len_state = sizeof(uint64_t), len_error = sizeof(uint64_t);
const int len_loc = sizeof(float) * 9;
const int len_head_loc = len_head + len_fun + len_len + len_time + len_state + len_error;
const int len_all = len_head + len_fun + len_len + len_time + len_state + len_error + len_loc;
const int len_end = 2;
const int len_usb_ai = 5, len_usb_slam = 4;
const float pi_1_2 = 1.570796f; // π/2
const float pi = 3.141593f;     // π
const float pi_3_2 = 4.712389f; // π*3/2
const float pi_2_1 = 6.283186f; // π*2
const float dt = 0.01f;
const int FRAME_LEN = sizeof(data_frame_t);
class SerialPub {
public:
    SerialPub(ros::NodeHandle node);
    ~SerialPub();
    bool handshake_bit, pps_bit, UsbStt;
    int sync_success_bit, last_sync_bit, all_sync_bit, sync_send_bit;
    uint8_t dir_cal, buf_save[204800], frame_buff_[204800], vec_loc[len_loc + 8];
    char log_md5_result[32];
    uint16_t map_service;
    uint32_t time_daysec, time_daynsec, upgrade_length, RcvDataLen;
    int device, up_file, usb_st, LoseDataNumb;
    float odom_X, odom_Y, Yaw_Angle, rtk_z, const_frame, const_angle, const_speed;
    double serial_tbefore, rtk_x, rtk_y, cam_sync_before, cam_sync_now;
    Rec_State_Un RecState;
    Alg_State rcv_alg_slm_state, snd_slm_state, rcv_ai_state, snd_ai_state;
    AlgError alg_error_time_;
    std::vector<uint8_t> vec_ai, vec_updown;
    std::string UsbName, m_log_, update_path_, rtk_path_, pst_sv, imu_sv, sub_addr, pub_addr;
    std::string ser_port, common_path, ver_path, log_pkg, log_pkg_img, log_pkg_zip, robot_fm_pkg;
    error_bit tx2_error;
    CSystemTime systime;
    Mlog_Save_Tik mlog_save_tik;
    Time_Count one_secend_count;
    TIME *my_time;
    Px30_Mode_Struct Px30_Mode_Contril;
    std::mutex AiSndMutex, PoseSndMutex;
    int log_mode;
    std_msgs::UInt8MultiArray AiUint8mltBuf, SlamUint8mltBuf;
    void RED_CFG();
    void PUB_MSG_MAIN();
    bool DEAL_TIME(std::vector<uint8_t>::iterator);
    bool DEAL_GPS(std::vector<uint8_t>::iterator);
    bool DEAL_IMU(std::vector<uint8_t>::iterator);
    bool DEAL_ODOM(std::vector<uint8_t>::iterator);
    bool USB_STT(void);
    void DEAL_DATA(void);
    void OPEN_CMNCT(const char *c_s);
    void RTK_IMU_ODOM(void);
    void PUB_MSG(void);
    void DATA_RCV(void);
    void PPS_RCV(void);
    void DATA_SND(void);
    void SYS_STT(void);
    void INTLZ_CONFIG();
    void PCA_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg);
    void LOC_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg);
    void UP_DWN_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg);
    void LOC_STT_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg);
    void AI_STT_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg);
    void BLOCK_THREAD();
    void SET_SYSTEM_TIME();

private:
    std::thread DataRcvThrd;
    std::thread DataSndThrd;
    std::thread DataSyncTimeThrd;
    std::thread DataSttThrd;
    std::thread DataDelThrd;
    std::thread LogSendThrd;
    sensor_msgs::NavSatFix FixMsg;
    geometry_msgs::TwistStamped TwsOdom;
    sensor_msgs::Imu SensImu;
    ros::Subscriber CldPotAiSub;
    ros::Subscriber LocationSub;
    ros::Subscriber LocSttSub;
    ros::Subscriber AiSttSub;
    ros::Subscriber UpDownSub;
    ros::Publisher FixGpsPub;
    ros::Publisher TwsOdomPub;
    ros::Publisher SensImuPub;
    ros::Publisher SndToAiPub;
    ros::Publisher SndToSlmPub;
};
void CP_FILE(const char *sourcefile, const char *destfile);
void TMN_PRT(uint8_t *buf, int buf_size);
void MSG_PRINTF(const std::string &msg);
void MSG_PRINTF_FLOAT(const std::string &msg_a, float msg);
void MSG_STATE(int bit, const std::string &msg_a);
void SAV_DATA(ofstream &drstm, string &pst_);
void SYS_RESTART_PTP();
string rosnode_manger(string function);
#endif
